#pragma once

// write by lbf_heu 2023/4/13

#include <iostream>
#include <fstream>
#include <random>
#include <ctime>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>


#include <ceres/ceres.h>

namespace Eigen{
    typedef Matrix<double, 7, 1> Vec7;
    typedef Matrix<double, 8, 1> Vec8;
    typedef Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> EMatrixXd;
};


Eigen::Quaterniond deltaQ(const Eigen::Vector3d &theta)
{
    Eigen::Quaterniond dq;
    Eigen::Matrix<double, 3, 1> half_theta = theta;
    half_theta /= (2.0);
    dq.w() = (1.0);
    dq.x() = half_theta.x();
    dq.y() = half_theta.y();
    dq.z() = half_theta.z();
    return dq;
}

// 定义四元数类型
class PoseLocalParameterization : public ceres::LocalParameterization
{
    virtual int GlobalSize() const { return 7; };
    virtual int LocalSize() const { return 6; };
    virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const
    {
        Eigen::Map<const Eigen::Vector3d> _p(x);
        Eigen::Map<const Eigen::Quaterniond> _q(x + 3);

        Eigen::Map<const Eigen::Vector3d> dp(delta);

        Eigen::Quaterniond dq = deltaQ(Eigen::Map<const Eigen::Vector3d>(delta + 3));

        Eigen::Map<Eigen::Vector3d> p(x_plus_delta);
        Eigen::Map<Eigen::Quaterniond> q(x_plus_delta + 3);

        p = _p + dp;
        q = (_q * dq).normalized();

        return true;
    }
    virtual bool ComputeJacobian(const double *x, double *jacobian) const
    {
        Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
        j.topRows<6>().setIdentity();
        j.bottomRows<1>().setZero();

        return true;
    }
};

// 仿真数据，每个坐标点和最近的左右2个点存在位姿关系
class Data{

public:
    double K1 = 6;
    double K2 = 8;
    double K3 = 0.1;

    double DeltaRot = 1.;

    double beginT = 0.;
    double endT = 15;
    double deltaT = 0.2;

    Eigen::Matrix3d initR = Eigen::Matrix3d::Identity();
    Eigen::Vector3d initT = Eigen::Vector3d::Zero();

    // 存放真值数据
    std::vector<Eigen::Vector3d> mvPs;
    std::vector<Eigen::Matrix3d> mvRots;
    std::vector<double> mTime;
    // 存放增量数据
    std::vector<std::vector<std::pair<int, Eigen::Vector3d>>> mvDps;
    std::vector<std::vector<std::pair<int, Eigen::Quaterniond>>> mvDqs;
    
    // 造点数据
    void makePointAndPos(){
        Eigen::Matrix3d ones = Eigen::Matrix3d::Identity();
        Eigen::Matrix3d nowRot = initR;
        for(double t = beginT; t < endT; t += deltaT){
            Eigen::Vector3d pos(K1 * sin(t), K2 * cos(t), K3 * sin(t) * cos(t));
            mvPs.push_back(pos);
            Eigen::AngleAxisd incRot(M_PI/180.* DeltaRot, ones.block<3, 1>(0, (mvRots.size()%3)));
            nowRot = nowRot * incRot.toRotationMatrix();
            mvRots.push_back(nowRot);
            
            mTime.push_back(t);
        }
        initR = mvRots.front();
        initT = mvPs.front();
    }
    // 制造观测数据  TODO: 增加噪声
    void makeMeasure(){
        
        mvDps.resize(mvPs.size());
        mvDqs.resize(mvPs.size());

        for(int i = 0; i < mvPs.size(); i++){
            
            auto & pi = mvPs[i];
            auto & qi = mvRots[i];

            for(int j = i-2; j <= i+2; j++){
                if((j < 0) || (j == i) || (j >= mvPs.size()))// 不超出索引范围 & 不与自己形成约束
                    continue;
                auto & pj = mvPs[j];
                auto & qj = mvRots[j];

                Eigen::Vector3d pjIni = qi.transpose() * pj - qi.transpose() * pi;
                Eigen::Quaterniond rjIni(qi.transpose() * qj);
                
                mvDps[i].push_back(std::make_pair(j, pjIni));
                mvDqs[i].push_back(std::make_pair(j, rjIni));
            }            
        }
    }

};

Eigen::Matrix3d skew(Eigen::Vector3d _in){
    Eigen::Matrix3d res;
    res << 0., -_in(2), _in(1),
            _in(2), 0., -_in(0),
            -_in(1), _in(0), 0.;
    return res;
}
// 计算右雅可比矩阵 copy from orb
Eigen::Matrix3d RightJacobianSO3(const Eigen::Vector3d & theta)
{
    Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
    const float d2 = theta[0] * theta[0] + theta[1] * theta[1] + theta[2] * theta[2];
    const float d = sqrt(d2);
    Eigen::Matrix3d W = skew(theta);
    if (d < 1e-5)
    {
        return I;
    }
    else
    {
        return I - W * (1.0 - cos(d)) / d2 + W * W * (d - sin(d)) / (d2 * d);
    }
}

// 计算右雅可比矩阵的逆
Eigen::Matrix3d InverseRightJacobianSO3(const Eigen::Vector3d & theta)
{
    Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
    const double d2 = theta[0] * theta[0] + theta[1] * theta[1] + theta[2] * theta[2];
    const double d = sqrt(d2);
    Eigen::Matrix3d W = skew(theta);

    if (d < 1e-5)
    {
        return I;
    }
    else
    {
        return I + W / 2 + W * W * (1.0 / d2 - (1.0 + cos(d)) / (2.0 * d * sin(d)));
    }
}

class myCostFunction : public ceres::SizedCostFunction<6, 7, 7> {
	public:

		myCostFunction(Eigen::Vector3d _dp, Eigen::Quaterniond _dq){
            mDp = _dp;
            mDq = _dq;
        }
		virtual ~myCostFunction() {}
		virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {    

            Eigen::Vector3d ti(parameters[0][0], parameters[0][1], parameters[0][2]);
            Eigen::Quaterniond qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
            Eigen::Vector3d tj(parameters[1][0], parameters[1][1], parameters[1][2]);
            Eigen::Quaterniond qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);

            Eigen::Map<Eigen::Matrix<double, 6, 1>> residual(residuals);
            
            // Eigen::Matrix3d cov = Eigen::Vector3d(mNoise*mNoise, mNoise*mNoise, mNoise*mNoise).asDiagonal();  
            Eigen::Matrix<double, 6, 6> sqrt_info;
            sqrt_info.setIdentity();
            //  = Eigen::LLT<Eigen::Matrix<double, 3, 3>>(cov.inverse()).matrixL().transpose();
            residual.block<3,1>(0, 0) = qi.conjugate() * tj - qi.conjugate() * ti - mDp;
            residual.block<3,1>(3, 0) = ((qi.conjugate() * qj).conjugate() * mDq).vec() * 2;// 转换成李代数
            
            residual = sqrt_info * residual;

            if(jacobians != NULL)
            {
                Eigen::Quaterniond temp = (qi.conjugate() * qj).conjugate() * mDq;
                if(jacobians[0] != NULL)// 对pi qi的导数
                { 
                    Eigen::Map<Eigen::Matrix<double, 6, 7, Eigen::RowMajor> > J_0(jacobians[0]);
                    J_0.setZero();
                    J_0.block<3, 3>(0, 0) = - qi.conjugate().matrix();
                    J_0.block<3, 3>(0, 3) = skew(qi.conjugate() * (tj - ti));
                    // 旋转残差对qi的导数
                    J_0.block<3, 3>(3, 3) = InverseRightJacobianSO3(temp.vec()*2) * mDq.conjugate();
                    J_0 = sqrt_info * J_0;
                }
                if(jacobians[1] != NULL)// 对pi qi的导数
                { 
                    Eigen::Map<Eigen::Matrix<double, 6, 7, Eigen::RowMajor> > J_1(jacobians[1]);
                    J_1.setZero();
                    J_1.block<3, 3>(0, 0) = qi.conjugate().matrix();
                    // 旋转残差对qi的导数
                    J_1.block<3, 3>(3, 3) = -InverseRightJacobianSO3(temp.vec()*2) * temp.conjugate();
                    J_1 = sqrt_info * J_1;
                }
                
            }  

            return true;
        
        }   
		
        Eigen::Vector3d mDp;
        Eigen::Quaterniond mDq;
};


class MarginalizationInfo{
public:
    // 保存边缘化后的雅可比矩阵和res
    Eigen::MatrixXd linearized_jacobians;
    Eigen::VectorXd linearized_residuals;
    
    // 保留边缘化的状态变量
    std::vector<double*> saveParams;

    MarginalizationInfo(int _windowSize){
        saveParams.resize(_windowSize-1);
        for(int i = 0; i < _windowSize-1; i++){
            saveParams[i] = new double[7];
        }
    }
    ~MarginalizationInfo(){
        for(auto & pointer : saveParams){
            delete[] pointer;
        }
    }
    // 打印 marge info的信息
    void logMargInfo(void){
        std::cout << "linerized_jacbobians size = " << linearized_jacobians.rows() << "," << linearized_jacobians.cols()
         << std::endl << linearized_jacobians << std::endl;
        std::cout << "linearized_residuals size = " << linearized_residuals.rows() << "," << linearized_residuals.cols()  
        << std::endl << linearized_residuals.transpose() << std::endl;
    }

};
// 定义边缘化因子
class MarginalizationFactor : public ceres::CostFunction
{
  public:
    MarginalizationFactor(MarginalizationInfo* _marginalization_info, std::vector<double *> params, int _windowSize){
        marginalization_info = _marginalization_info;
        for(int i = 0; i < params.size(); i++){
            indexMap[params[i]] = i * 7;
        }
        mWindowSize = _windowSize;

        for (int i = 0; i < _windowSize-1; i++){
            mutable_parameter_block_sizes()->push_back(7);
        }
        set_num_residuals(_windowSize*7-7);
    }
    // 对 0 - 8帧 均存在residual 和 jacobian
    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const{
        
        auto & eigenRes = marginalization_info->linearized_residuals;
        auto & eigenJacobian = marginalization_info->linearized_jacobians;
        Eigen::VectorXd dx(mWindowSize*7 - 7);
        dx.setZero();
        for(int i = 0; i < mWindowSize-1; i++){
            Eigen::VectorXd x = Eigen::Map<const Eigen::VectorXd>(parameters[i], 7);
            Eigen::VectorXd x0 = Eigen::Map<const Eigen::VectorXd>(marginalization_info->saveParams[i], 7);
            dx.segment<3>(i * 7 + 0) = x.head<3>() - x0.head<3>();
            dx.segment<3>(i * 7 + 3) = 2.0 * (Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec();
            if (!((Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).w() >= 0)){
                dx.segment<3>(i * 7 + 3) = 2.0 * -(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec();
            }
        }
        Eigen::Map<Eigen::VectorXd>(residuals, mWindowSize*7 - 7) = marginalization_info->linearized_residuals + marginalization_info->linearized_jacobians * dx;
        if (jacobians)// 对残差求所有的雅可比矩阵
        {

            for (int j = 0; j < mWindowSize-1; j++)// 一共9个残差
            {
                if (jacobians[j])
                {
                    int size = 7;
                    int idx = j*7;
                    // jacobian 是 6 × 9 × 7 的
                    Eigen::Map<Eigen::EMatrixXd> jacobian(jacobians[j], 7, (mWindowSize-1)*7);
                    jacobian.setZero();
                    jacobian = marginalization_info->linearized_jacobians.block(idx, 0, 7, (mWindowSize-1)*7);
                }
            }
        }        
        return true;

    }

    MarginalizationInfo* marginalization_info;// 里面保存不同残差对不同状态变量的jacobian
    std::map<double*, int> indexMap;
    int mWindowSize = 10;
};

// 与参数块相连的结构体
struct residualAndParmas{
    ceres::CostFunction *mCost_function;
    std::vector<double *> mParams;
    int err_size = 6;// 默认残差维度是6
};


// 定义代价函数的计算
class ceres_solver
{
private:
    const int windowSize = 10;
    
    Data datar;
    
    // log file
    std::ofstream outTrue;
    std::ofstream outFullBA;
    std::ofstream outWindowBa;
    std::ofstream fusionFile;

    // opt params
    std::vector<double*> optParams;  

    // out 
    std::vector<Eigen::Vec8> outVec;
public:
    // 进行全局ba的过程
    void fullBA(){
        // 构建问题
        ceres::Problem::Options problem_options;
        ceres::Problem problem(problem_options);

        optParams.resize(datar.mvPs.size());      
        // 添加参数块
        for(auto & pointer : optParams){
            pointer = new double[7];
            pointer[0] = 0;
            pointer[1] = 0;
            pointer[2] = 0;
            pointer[3] = 0;
            pointer[4] = 0;
            pointer[5] = 0;
            pointer[6] = 1;
            ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
            problem.AddParameterBlock(pointer, 7, local_parameterization);
        }
        optParams[0][0] = datar.initT[0];
        optParams[0][1] = datar.initT[1];
        optParams[0][2] = datar.initT[2];
        Eigen::Quaterniond rot(datar.initR);
        optParams[0][3] = rot.x();
        optParams[0][4] = rot.y();
        optParams[0][5] = rot.z();
        optParams[0][6] = rot.w();
        problem.SetParameterBlockConstant(optParams[0]);
        // 添加代价块
        for(int i = 0; i < datar.mvDps.size(); i++){
            for(int j = 0; j < datar.mvDps[i].size(); j++){
                ceres::CostFunction *cost_function = new myCostFunction(datar.mvDps[i][j].second, datar.mvDqs[i][j].second);  
                problem.AddResidualBlock(cost_function, NULL, optParams[i], optParams[datar.mvDps[i][j].first]);
            }
        }


        ceres::Solver::Options options;
        options.linear_solver_type = ceres::DENSE_QR;
        options.minimizer_progress_to_stdout = false;
        options.check_gradients = false;
        options.gradient_check_relative_precision = 1e-4;
        ceres::Solver::Summary summary;

        ceres::Solve(options, &problem, &summary);
        std::cout << std::endl;
    }
    
    // 构建AB矩阵
    void buildABMatrix(Eigen::MatrixXd & A, Eigen::VectorXd & B, std::vector<double *> & windowOpParams, std::vector<residualAndParmas> & ResidualBlocks){
        // 构建index索引
        std::map<double*, int> paramIndex;
        for(int i = 0; i < windowOpParams.size(); i++){
            paramIndex[windowOpParams[i]] = i * 7;
        }
        // 一共 windowsize 帧率 边缘化第0帧  m = 7, n = 7 * (windowsize - 1)
        int pos = 7 * windowSize;
        A = Eigen::MatrixXd(pos, pos);// A是 pos pos大小的
        B = Eigen::VectorXd(pos);// b是pos大小的
        A.setZero();
        B.setZero();

        // 遍历所有的factor
        for(auto & resBlock : ResidualBlocks){
            int res_size = resBlock.err_size;
            double * res = new double[res_size];
            double ** jacobian = new double*[resBlock.mParams.size()];
            double ** params = new double*[resBlock.mParams.size()];
            for(int i = 0; i < resBlock.mParams.size(); i++){
                jacobian[i] = new double[res_size * 7];
                params[i] = resBlock.mParams[i];
            }
            resBlock.mCost_function->Evaluate(params, res, jacobian);// params res jacobian
            // 遍历所有的param
            Eigen::Map<Eigen::EMatrixXd> resEigen(res, res_size, 1);
            for(int i = 0; i < resBlock.mParams.size(); i++){
                Eigen::Map<Eigen::EMatrixXd> Ji(jacobian[i], res_size, 7);
                int indexi = paramIndex[resBlock.mParams[i]]; // 当前雅可比矩阵在矩阵索引中的位置
                for(int j = i; j < resBlock.mParams.size(); j++){
                    Eigen::Map<Eigen::Matrix<double, 6, 7, Eigen::RowMajor> > Jj(jacobian[j]);
                    int indexj = paramIndex[resBlock.mParams[j]]; // 当前雅可比矩阵在矩阵索引中的位置
                    A.block<7, 7>(indexi, indexj) = Ji.transpose() * Jj;
                    B.block<7, 1>(indexi, 0) = -Ji.transpose() * resEigen;
                } 
            }
        }

        for(int i = 0; i < pos; i++){
            for(int j = i+1; j < pos; j++){
                A(j, i) = A(i, j);
            }
        }
    }
    // 进行舒尔补
    void schur(Eigen::MatrixXd & A, Eigen::VectorXd & B, Eigen::MatrixXd & newA, Eigen::VectorXd & newB){
        int m = 7;
        int n = windowSize * 7 - 7;
        double eps = 1e-7;
        Eigen::MatrixXd Amm = 0.5 * (A.block(0, 0, m, m) + A.block(0, 0, m, m).transpose());
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm);
        Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd((saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() * saes.eigenvectors().transpose();
        Eigen::VectorXd bmm = B.segment(0, m);
        Eigen::MatrixXd Amr = A.block(0, m, m, n);
        Eigen::MatrixXd Arm = A.block(m, 0, n, m);
        Eigen::MatrixXd Arr = A.block(m, m, n, n);
        Eigen::VectorXd brr = B.segment(m, n);
        // 使用舒尔补进行边缘化
        newA = Arr - Arm * Amm_inv * Amr;// 新的A
        newB = brr - Arm * Amm_inv * bmm;// 新的B
    }
    // 从AB中恢复 J 与 res
    void getJacobianAndRes(Eigen::MatrixXd & A, Eigen::VectorXd & B, Eigen::MatrixXd & linearized_jacobians, Eigen::VectorXd & linearized_residuals){
        double eps = 1e-7;
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(A);// 对A进行VDV^-1的分解  D是一个对角矩阵  V被认为了J^T
        Eigen::VectorXd S = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array(), 0));// 非0块不要
        Eigen::VectorXd S_inv = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array().inverse(), 0));// V逆矩阵也是
        Eigen::VectorXd S_sqrt = S.cwiseSqrt();
        Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt();
        linearized_jacobians = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();// jacobin  J^T × 
        linearized_residuals = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * B;
    }
    // 进行滑动窗口BA
    void windowBA(){
        // windowSize 里的一起优化  TODO: 把这里的这部分代码和上面进行统一起来
        std::vector<double *> windowOpParams(windowSize);
        std::vector<double> timeStemp(windowSize);

        for(auto & pointer : windowOpParams){
            pointer = new double[7];
            pointer[0] = 0;
            pointer[1] = 0;
            pointer[2] = 0;
            pointer[3] = 0;
            pointer[4] = 0;
            pointer[5] = 0;
            pointer[6] = 1;
        }
        windowOpParams[0][0] = datar.initT[0];
        windowOpParams[0][1] = datar.initT[1];
        windowOpParams[0][2] = datar.initT[2];
        Eigen::Quaterniond rot(datar.initR);
        windowOpParams[0][3] = rot.x();
        windowOpParams[0][4] = rot.y();
        windowOpParams[0][5] = rot.z();
        windowOpParams[0][6] = rot.w();

        // 边缘化用到的语料
        MarginalizationInfo * marger;
        marger = nullptr;
        for(int i = 0; i < datar.mvDps.size()-windowSize; i++){
            std::cout << "windonw begin num :" << i << std::endl;
            // 构建问题
            ceres::Problem::Options problem_options;
            ceres::Problem problem(problem_options);
            // 添加参数块
            for(int k = 0; k < windowSize; k++){
                ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
                problem.AddParameterBlock(windowOpParams[k], 7, local_parameterization);
            }

            if(marger == nullptr)// 没有边缘化的时候固定
                problem.SetParameterBlockConstant(windowOpParams[0]);
            else{// 添加边缘化残差块
                // marger->logMargInfo();
                ceres::CostFunction *cost_function = new MarginalizationFactor(marger, windowOpParams, windowSize);
                std::vector<double*> savedParams = windowOpParams;
                savedParams.resize(windowSize-1);
                problem.AddResidualBlock(cost_function, NULL, savedParams);
            }
            
            // 添加残差块
            for(int windowNum = i; windowNum < i+windowSize; windowNum++){
                timeStemp[windowNum - i] = datar.mTime[windowNum];// 传递时间戳
                // 对连接关系进行遍历
                for(int j = 0; j < datar.mvDps[windowNum].size(); j++){

                    int iIndex = windowNum - i;
                    int jIndex = datar.mvDps[windowNum][j].first - i;

                    if(jIndex < 0 || jIndex >= windowSize)
                        continue;
                    ceres::CostFunction *cost_function = new myCostFunction(datar.mvDps[windowNum][j].second, datar.mvDqs[windowNum][j].second);  
                    problem.AddResidualBlock(cost_function, NULL, windowOpParams[iIndex], windowOpParams[jIndex]);
                }
            }
            // 优化
            ceres::Solver::Options options;
            options.linear_solver_type = ceres::DENSE_QR;
            options.minimizer_progress_to_stdout = false;
            options.check_gradients = false;
            options.gradient_check_relative_precision = 1e-4;
            ceres::Solver::Summary summary;
            ceres::Solve(options, &problem, &summary);

            // 保留最老帧的消息
            {
                Eigen::Vec8 in;// datar.mTime[i]
                in << timeStemp[0], windowOpParams[0][0], windowOpParams[0][1], windowOpParams[0][2], 
                windowOpParams[0][3], windowOpParams[0][4], windowOpParams[0][5], windowOpParams[0][6];
                outVec.push_back(in);
            }
            // 边缘化老帧 四板斧 矩阵多大 计算残差和雅可比 残差 怎莫用
            {
                // 收集当前的factor
                std::vector<residualAndParmas> ResidualBlocks;
                for(int windowNum = i; windowNum < i+windowSize; windowNum++){
                    for(int j = 0; j < datar.mvDps[windowNum].size(); j++){
                        int iIndex = windowNum - i;
                        int jIndex = datar.mvDps[windowNum][j].first - i;
                        if(jIndex < 0 || jIndex >= windowSize)
                            continue;
                        residualAndParmas temp;
                        temp.mCost_function = new myCostFunction(datar.mvDps[windowNum][j].second, datar.mvDqs[windowNum][j].second);  
                        temp.mParams.push_back(windowOpParams[iIndex]);
                        temp.mParams.push_back(windowOpParams[jIndex]);
                        temp.err_size = 6;
                        ResidualBlocks.push_back(temp);
                    }
                }

                // 收集上一次marge的factor
                if(marger != nullptr){
                    residualAndParmas temp;
                    temp.mCost_function = new MarginalizationFactor(marger, windowOpParams, windowSize);
                    for(int h = 0; h < windowSize-1; h++){
                        temp.mParams.push_back(windowOpParams[h]);// 上一次的边缘化的参数块和 0-8相关
                    }
                    temp.err_size = 63;
                    ResidualBlocks.push_back(temp);
                }

                // 进行舒尔补并计算新的雅可比矩阵以及残差
                MarginalizationInfo* newMarger = new MarginalizationInfo(windowSize);
                Eigen::MatrixXd A, newA;
                Eigen::VectorXd B, newB;
                buildABMatrix(A, B, windowOpParams, ResidualBlocks);
                schur(A, B, newA, newB);
                getJacobianAndRes(newA, newB, newMarger->linearized_jacobians, newMarger->linearized_residuals);
                // log matrix
                // std::cout << "A size = " << logMatrixSize(A) << std::endl;
                // std::cout << "B size = " << logMatrixSize(B) << std::endl;
                // std::cout << "newA size = " << logMatrixSize(newA) << std::endl;
                // std::cout << "newB size = " << logMatrixSize(newB) << std::endl;
                // std::cout << "linearized_jacobians size = " << logMatrixSize(newMarger->linearized_jacobians) << std::endl;
                // std::cout << "linearized_residuals size = " << logMatrixSize(newMarger->linearized_residuals) << std::endl;
                // std::cout << "A.matrix" << std::endl << A << std::endl;
                // std::cout << "B.matrix" << std::endl << B << std::endl;
                // std::cout << "newA.matrix" << std::endl << newA << std::endl;
                // std::cout << "newB.matrix" << std::endl << newB << std::endl;

                // 保存当前的状态
                for(int windowNum = 0; windowNum < windowSize-1; windowNum++){
                    for(int k = 0; k < 7; k++)
                        newMarger->saveParams[windowNum][k] = windowOpParams[windowNum+1][k];
                }
                marger = newMarger;
            }
            // 窗口滑动
            for(int windowNum = 0; windowNum < windowSize-1; windowNum++){
                for(int k = 0; k < 7; k++){
                    windowOpParams[windowNum][k] = windowOpParams[windowNum+1][k];
                }
                timeStemp[windowNum] = timeStemp[windowNum+1];
                    
            }

        }
        // 滑动窗口的其他帧塞入进来
        for(int i = 0; i < windowSize-1; i++){
            Eigen::Vec8 in;
            in << timeStemp[i], windowOpParams[i][0], windowOpParams[i][1], windowOpParams[i][2], 
            windowOpParams[i][3], windowOpParams[i][4], windowOpParams[i][5], windowOpParams[i][6];
            outVec.push_back(in);
        }
        for(int i = 0; i < outVec.size(); i++){
            outWindowBa << outVec[i](0, 0) << " "
            << outVec[i](1, 0) << " "
            << outVec[i](2, 0) << " "
            << outVec[i](3, 0) << " "
            << outVec[i](4, 0) << " "
            << outVec[i](5, 0) << " "
            << outVec[i](6, 0) << " "
            << outVec[i](7, 0) << std::endl;
        }
        
    }
    // TODO: 用templt代替
    std::string logMatrixSize(Eigen::MatrixXd & A){
        std::string res;
        res = " (" + std::to_string(A.rows()) + "," + std::to_string(A.cols()) + ") ";
        return res;
    }

    std::string logMatrixSize(Eigen::VectorXd & A){
        std::string res;
        res = " (" + std::to_string(A.rows()) + "," + std::to_string(A.cols()) + ") ";
        return res;
    }

    ceres_solver(){

        // 产生数据
        datar.makePointAndPos();
        datar.makeMeasure();

        outTrue.open("true.txt");
        outFullBA.open("fullBA.txt");
        outWindowBa.open("outWindowBa.txt");
        fusionFile.open("fusion.txt");
        
    }
    // log成tum格式
    void logPoints(){

        for(int i = 0; i < datar.mvPs.size(); i++){
            Eigen::Quaterniond rot(datar.mvRots[i]);
            outTrue << datar.mTime[i] << " "
            << datar.mvPs[i][0] << " "
            << datar.mvPs[i][1] << " "
            << datar.mvPs[i][2] << " "
            << rot.x() << " "
            << rot.y() << " "
            << rot.z() << " "
            << rot.w() << std::endl;

            outFullBA << datar.mTime[i] << " "
            << optParams[i][0] << " "
            << optParams[i][1] << " "
            << optParams[i][2] << " "
            << optParams[i][3] << " "
            << optParams[i][4] << " "
            << optParams[i][5] << " "
            << optParams[i][6] << std::endl;

        }
        
    }

    ~ceres_solver(){}
};


